

#include<math.h>
#include"algfloat.h"

void AFScale(float scalar, float* v) 
{
  v[0]*=scalar;
  v[1]*=scalar;
  v[2]*=scalar;
}

void AFSub ( float * v1, float * v2, float * res)
{
  res[0] = v1[0] - v2[0];
  res[1] = v1[1] - v2[1];
  res[2] = v1[2] - v2[2];
  res[3] = v1[3];
}

void AFAdd ( float * v1, float * v2, float * res)
{
  res[0] = v1[0] + v2[0];
  res[1] = v1[1] + v2[1];
  res[2] = v1[2] + v2[2];
  res[3] = v1[3];
}

float AFNorm(float* vector) 
{
  return (float) sqrt(vector[0]*vector[0] + vector[1]*vector[1]+ vector[2]*vector[2]);
}

void AFUnit (float * v)
{
  float n = AFNorm(v);
  if ( n > 1e-9 ) 
    AFScale(1/n,v);
}

float AFDot (float * v1, float * v2 )
{
  return v1[0]*v2[0] + v1[1]*v2[1] + v1[2]*v2[2];
}

void AFCopy (float * v, float* cp)
{
  cp[0]=v[0]; cp[1]=v[1]; cp[2]=v[2]; cp[3]=v[3]; 
}

void AFProj( float * ofVector, float * ontoVector, float  * res ) 
{
	float fNewScale = AFDot(ofVector, ontoVector)/AFDot(ontoVector,ontoVector);
  AFCopy(ontoVector,res);
	AFScale(fNewScale , res);
}

void AFReflect( float* ofVector, float* aroundVector, float* res ) 
{
  float vProj[4];
  AFProj(ofVector,aroundVector,vProj);
  float vIncrH[4];
  AFSub(vProj,ofVector,vIncrH);
  AFAdd(vProj, vIncrH,res);
}

void AFMatXVec (float * mat, float * vec, float * res)
{
  res[0] = mat[0]*vec[0] + mat[4]*vec[1] + mat[8]*vec[2]  + mat[12]*vec[3];
  res[1] = mat[1]*vec[0] + mat[5]*vec[1] + mat[9]*vec[2]  + mat[13]*vec[3];
  res[2] = mat[2]*vec[0] + mat[6]*vec[1] + mat[10]*vec[2] + mat[14]*vec[3];
  res[3] = mat[3]*vec[0] + mat[7]*vec[1] + mat[11]*vec[2] + mat[15]*vec[3];
}
